import numpy as np
import csv
import pandas as pd
import cv2
import math
import mediapipe as mp


def distance(spot1: list, spot2: list):
    """
    计算两点的距离
    """
    return math.sqrt((spot2[0]-spot1[0])**2 + (spot2[1]-spot1[1])**2 + (spot2[2]-spot1[2])**2)


def fd(Hlist: dict):
    """
    传入一个handlist, 我们来处理各个手指节点的距离
    """
    DistanceList = np.array([
        [round(distance(HandList[4], HandList[0]), 4), round(distance(HandList[4], HandList[1]), 4),
         round(distance(HandList[4], HandList[2]), 4), round(distance(HandList[4], HandList[3]), 4),
         round(distance(HandList[4], HandList[4]), 4), round(distance(HandList[4], HandList[5]), 4),
         round(distance(HandList[4], HandList[6]), 4), round(distance(HandList[4], HandList[7]), 4),
         round(distance(HandList[4], HandList[8]), 4), round(distance(HandList[4], HandList[9]), 4),
         round(distance(HandList[4], HandList[10]), 4), round(distance(HandList[4], HandList[11]), 4),
         round(distance(HandList[4], HandList[12]), 4), round(distance(HandList[4], HandList[13]), 4),
         round(distance(HandList[4], HandList[14]), 4), round(distance(HandList[4], HandList[15]), 4),
         round(distance(HandList[4], HandList[16]), 4), round(distance(HandList[4], HandList[17]), 4),
         round(distance(HandList[4], HandList[18]), 4), round(distance(HandList[4], HandList[19]), 4),
         round(distance(HandList[4], HandList[20]), 4)],
        [round(distance(HandList[8], HandList[0]), 4), round(distance(HandList[8], HandList[1]), 4),
         round(distance(HandList[8], HandList[2]), 4), round(distance(HandList[8], HandList[3]), 4),
         round(distance(HandList[8], HandList[4]), 4), round(distance(HandList[8], HandList[5]), 4),
         round(distance(HandList[8], HandList[6]), 4), round(distance(HandList[8], HandList[7]), 4),
         round(distance(HandList[8], HandList[8]), 4), round(distance(HandList[8], HandList[9]), 4),
         round(distance(HandList[8], HandList[10]), 4), round(distance(HandList[8], HandList[11]), 4),
         round(distance(HandList[8], HandList[12]), 4), round(distance(HandList[8], HandList[13]), 4),
         round(distance(HandList[8], HandList[14]), 4), round(distance(HandList[8], HandList[15]), 4),
         round(distance(HandList[8], HandList[16]), 4), round(distance(HandList[8], HandList[17]), 4),
         round(distance(HandList[8], HandList[18]), 4), round(distance(HandList[8], HandList[19]), 4),
         round(distance(HandList[8], HandList[20]), 4)],
        [round(distance(HandList[12], HandList[0]), 4), round(distance(HandList[12], HandList[1]), 4),
         round(distance(HandList[12], HandList[2]), 4), round(distance(HandList[12], HandList[3]), 4),
         round(distance(HandList[12], HandList[4]), 4), round(distance(HandList[12], HandList[5]), 4),
         round(distance(HandList[12], HandList[6]), 4), round(distance(HandList[12], HandList[7]), 4),
         round(distance(HandList[12], HandList[8]), 4), round(distance(HandList[12], HandList[9]), 4),
         round(distance(HandList[12], HandList[10]), 4), round(distance(HandList[12], HandList[11]), 4),
         round(distance(HandList[12], HandList[12]), 4), round(distance(HandList[12], HandList[13]), 4),
         round(distance(HandList[12], HandList[14]), 4), round(distance(HandList[12], HandList[15]), 4),
         round(distance(HandList[12], HandList[16]), 4), round(distance(HandList[12], HandList[17]), 4),
         round(distance(HandList[12], HandList[18]), 4), round(distance(HandList[12], HandList[19]), 4),
         round(distance(HandList[12], HandList[20]), 4)],
        [round(distance(HandList[16], HandList[0]), 4), round(distance(HandList[16], HandList[1]), 4),
         round(distance(HandList[16], HandList[2]), 4), round(distance(HandList[16], HandList[3]), 4),
         round(distance(HandList[16], HandList[4]), 4), round(distance(HandList[16], HandList[5]), 4),
         round(distance(HandList[16], HandList[6]), 4), round(distance(HandList[16], HandList[7]), 4),
         round(distance(HandList[16], HandList[8]), 4), round(distance(HandList[16], HandList[9]), 4),
         round(distance(HandList[16], HandList[10]), 4), round(distance(HandList[16], HandList[11]), 4),
         round(distance(HandList[16], HandList[12]), 4), round(distance(HandList[16], HandList[13]), 4),
         round(distance(HandList[16], HandList[14]), 4), round(distance(HandList[16], HandList[15]), 4),
         round(distance(HandList[16], HandList[16]), 4), round(distance(HandList[16], HandList[17]), 4),
         round(distance(HandList[16], HandList[18]), 4), round(distance(HandList[16], HandList[19]), 4),
         round(distance(HandList[16], HandList[20]), 4)],
        [round(distance(HandList[20], HandList[0]), 4), round(distance(HandList[20], HandList[1]), 4),
         round(distance(HandList[20], HandList[2]), 4), round(distance(HandList[20], HandList[3]), 4),
         round(distance(HandList[20], HandList[4]), 4), round(distance(HandList[20], HandList[5]), 4),
         round(distance(HandList[20], HandList[6]), 4), round(distance(HandList[20], HandList[7]), 4),
         round(distance(HandList[20], HandList[8]), 4), round(distance(HandList[20], HandList[9]), 4),
         round(distance(HandList[20], HandList[10]), 4), round(distance(HandList[20], HandList[11]), 4),
         round(distance(HandList[20], HandList[12]), 4), round(distance(HandList[20], HandList[13]), 4),
         round(distance(HandList[20], HandList[14]), 4), round(distance(HandList[20], HandList[15]), 4),
         round(distance(HandList[20], HandList[16]), 4), round(distance(HandList[20], HandList[17]), 4),
         round(distance(HandList[20], HandList[18]), 4), round(distance(HandList[20], HandList[19]), 4),
         round(distance(HandList[20], HandList[20]), 4)]])
    return DistanceList


if __name__ == "__main__":

    cap = cv2.VideoCapture(0)

    a = mp.solutions.hands
    hands = a.Hands()
    HandDraw = mp.solutions.drawing_utils
    HandList = {}
    f = open("handtest.csv", "a", encoding="utf-8")
    writer = csv.writer(f)
    while True:
        suc, img = cap.read()

        rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        result = hands.process(rgb)
        if result.multi_hand_landmarks:
            for i in result.multi_hand_landmarks:
                for Id, lm in enumerate(i.landmark):
                      HandList[Id] = [lm.x, lm.y, lm.z]
                d = fd(HandList)

                d = np.array(d.flatten()).tolist()
                print(d)
                writer.writerow([2, d])
                HandList = {}
                HandDraw.draw_landmarks(img, i, a.HAND_CONNECTIONS)
        cv2.imshow("hand", img)
        cv2.waitKey(1)
    f.close()
